#include "src/common/common.h"

int main(void)
{
  // std::vector<std::pair<float, float> > seg_start_points;
  // seg_start_points.emplace_back(1,2);
  // seg_start_points.emplace_back(3,4);
  // DumpMsg(123, seg_start_points.data(), seg_start_points.size() * 2, "a");
  // float a[10];
  // float *a_ptr = &a[0];
  // for (size_t i = 0; i < 10; i++) {
  //     a_ptr++ = 1;
  // }
  // for (int i = 0; i < 10; i++) {
  //     std::cout << "a" << i << ":" << a[i] << " ";
  // }
  // std::vector<std::pair<float, float> > seg_start_points;
  // std::vector<std::pair<float, float> > seg_start_points2;
  // seg_start_points.emplace_back(1,2);
  // seg_start_points.emplace_back(3,4);
  // seg_start_points2 = seg_start_points;
  // seg_start_points.emplace_back(5,6);
  // for (auto p : seg_start_points2) {
  //     std::cout << "p" << p.first << p.second;
  // }
  // int p = 0;
  // for (size_t i = 0; i < 10; i++) {
  //   ++p;
  //   std::cout << p << std::endl;
  // }
  // std::cout << FLT_MAX << std::endl;
  // if (top1_traj.trajectories(point_num - 1).trajectory().y() >
  //         std::abs(line_y) ||
  //     top1_traj.trajectories(point_num - 1).trajectory().y() <
  //         -std::abs(line_y)) {
  // }
  // float a{1, 2, 3, 4};
  // float b = PythonMod(-8.32581, 2 * M_PI);
  // float c = b==PythonMod(-8.32581, 2 * M_PI)?0:1;
  // std::cout << "b:" << b << std::endl;
  // std::cout << "c:" << c << std::endl;

  // float data[10];
  // int size = 10 * sizeof(float);

  // memset(data, 0, size);
  // for(int i=0;i<10;i++){
  //     std::cout<<data[i]<<" ";
  // }
  // DumpMsg(100, reinterpret_cast<float *>(data), size, "data");
  // // DumpMsg(101, &data, size, "data");

  // std::cout << "size:" << size << " data size:" << sizeof(data) << std::endl;
  // std::cout << data << std::endl;
  // std::shared_ptr<Lanes> lane_ptr = std::make_shared<Lanes>();
  // std::cout << "lane_ptr:" << lane_ptr << std::endl;
  // lane_ptr->id = 11;
  // std::cout << "lane_id:" << lane_ptr->id << std::endl;
  // std::cout << "================>" << std::endl;
  // // std::vector<int> array{1,2,3};
  // // auto& l = array.back();
  // // int h;
  // // h = l;
  // // h = 12;
  // // array.back() = h;
  // // std::cout << array.back() << "," << l;
  // std::vector<float> data1;
  // data1.emplace_back(1);
  // data1.emplace_back(2);
  // DumpMsg(111, data1.data(), data1.size() * sizeof(float), "data");
  // bool dt = true;
  // for (size_t i = 0; i < 10; ++i) {
  //   for (size_t j = 0; j < 10; ++j) {
  //     if (dt) {
  //       if (j == 5) {
  //       continue;
  //       }
  //     }
  //     std::cout << "i:" << i << " j:" << j << "\n";
  //   }
  // }
  std::cout << "================>" << std::endl;
  std::vector<float> array1;
  std::vector<float> array2;
  array1 = {3.1, -3.1, -1.5, 0, 1.5, 3.1};
  Unwrap(array1, array2);
  for (size_t i = 0; i < array2.size(); ++i)
  {
    std::cout << array2[i] << " ";
  }
  std::cout << "================>" << std::endl;
  float PT[5] = {1, 2, 3, 4, 5};
  float *ptr = PT;
  // *ptr++ = *ptr;
  *ptr = *(ptr + 1);
  ptr++;
  std::cout << *ptr << std::endl;
  for (size_t i = 0; i < 5; ++i)
  {
    std::cout << PT[i] << " ";
  }
  std::cout << "================>" << std::endl;
  std::vector<std::vector<int>> test_data;
  std::vector<int> tmp1 = {1, 2, 4};
  std::vector<int> tmp2 = {1};
  test_data.emplace_back(tmp1);
  test_data.emplace_back(tmp1);
  test_data.emplace_back(tmp1);
  test_data.back() = tmp2;
  // std::sort(test_data.begin(), test_data.end(), [&](int a, int b) {
  //   return a < b;
  // });
  for (size_t i = 0; i < test_data.size(); ++i)
  {
    std::cout << test_data[i][0] << " ";
  }
  float x, y;

  TranslationAndRotation(0.0f, 0.0f, float(M_PI / 2), 1.0f, 0.0f, x, y);

  std::cout << "x:" << x << " y:" << y << std::endl;
  TrackingDifferentiator td_;
  td_.Init(1.0, 0.1, 0.0);
  double han = td_.Fhan(td_.x1_ - 10, td_.x2_);
  std::cout << "han:" << han << std::endl;
  std::vector<Dignose> dignoses{2};
  auto &dig = dignoses[0];
  auto b = dig.b;
  b.insert(b.begin(), tmp1.begin(), tmp1.end());
  ChangeValue(dig, dig.a);
  std::cout << "dig[0]:" << dignoses[0].a << "dig[0].b:" << dignoses[0].b.size()
            << std::endl;
  std::cout << PythonMod(-5 * M_PI / 2, M_PI);
  std::vector<float> d = ExtendNewPoint(0.0f, 0.0f, 0.0f, 1.0f, 1.0f);
  std::cout << "ExtendNewPoint d:" << d[0] << "," << d[1] << std::endl;
  CrossPoint(0.0f, 0.0f, 1.0f, 1.0f, 3.0f, 8.0f, 1.0f, 2.0f, x, y);
  std::cout << "cross point:" << x << "," << y << std::endl;
  std::vector<int> out_b;
  dig.b.emplace_back();
  dig.b.back() = 1;
  std::cout << "dig_b:" << dig.b.back() << std::endl;
  ConstReferenceToReference(dig, out_b);
  std::cout << "out_b:" << out_b.back() << std::endl;
  out_b.back() = 2;
  std::cout << "out_b:" << out_b.back() << std::endl;
  std::cout << "================>" << std::endl;
  LinePoint<float> p1, p2, p3, p4, cross_point;
  p1.x = 0;
  p1.y = 0;
  p1.time = 1689910461403;
  p2.x = 2;
  p2.y = 2;
  p2.time = 1689910461903;
  p3.x = 2;
  p3.y = 0;
  p3.time = 1689910462403;
  p4.x = 1;
  p4.y = 1.1;
  p4.time = 1689910462903;
  if (RapidRejExper(p1, p2, p3, p4) && Cross(p1, p2, p3, p4))
  {
    std::cout << "is cross" << std::endl;
  }
  else
  {
    std::cout << "do not cross" << std::endl;
  }
  if (CosTheta(p1, p2, p4, p3) > 0.0f &&
      CrossPoint(p1.x, p2.x, p3.x, p4.x, p1.y, p2.y, p3.y, p4.y, cross_point.x,
                 cross_point.y))
  {
    std::cout << "射线 cross:" << cross_point.x << "," << cross_point.y
              << std::endl;
  }
  else
  {
    std::cout << "射线 do not cross" << std::endl;
  }
  int64_t t = 100000;
  int64_t t1 = 100500;
  float step = 500;
  int64_t time = t + step;
  std::cout << static_cast<float>(t) / static_cast<float>(t1) << std::endl;
  std::vector<LinePoint<float>> trajectory;
  trajectory.resize(4);
  trajectory[0] = p1;
  trajectory[1] = p2;
  trajectory[2] = p3;
  trajectory[3] = p4;
  int64_t lerp_time = 1689910461603;
  int i_1nd, i_2nd = 0;
  float weight = 0.0f;
  GetIntervalIndexByT(trajectory, lerp_time, &i_1nd, &i_2nd, &weight,
                      NULL);
  std::cout << "i_1nd:" << i_1nd << " i_2nd:" << i_2nd << " weight:" << weight << std::endl;
  float theta = LinearInterpolationTheta(M_PI / 2, M_PI / 2 + 0.1, 0.1);
  std::cout << "theta:" << theta << std::endl;
  float x1, y1, yaw1, v1, yaw_rate1;
  float yaw0 = static_cast<float>(M_PI);
  KinematicModelPrediction(0.0f, 0.0f, yaw0, 1.0f, 0.0f, 1.0f, x1, y1, yaw1, v1, yaw_rate1);
  std::cout << "x1:" << static_cast<double>(x1) << " y1:" << y1 << " yaw1:" << yaw1 << " v1:" << v1 << " yaw_rate1:" << yaw_rate1 << std::endl;
  int64_t ti = 10;
  step = 9.9f;
  bool res = ti > step;
  std::cout << "res:" << res << std::endl;

  std::unordered_map<std::string, PredictionInfo> prediction_infos_;
  Object object;
  object.id == "1";
  PredictionInfo pred_info(object);
  std::cout << "111";
  prediction_infos_["1"] = pred_info;
  std::cout << "222";
  if (prediction_infos_.find(object.id) != prediction_infos_.end())
  {
    if (prediction_infos_[object.id].pred_bind_lane ==
        prediction_infos_[object.id].main_bind_lane - 1){}
    // {
    //   // object.lane_behavior = LaneBehavior::LANE_BEHAVIOR_LEFT;
    // }
  }
  std::cout << "aaaaa" << std::endl;
  return 0;
}

// void BindObstacleWithLane(std::vector<ObsInfo> lcf_obs) {}